
#include "Ano_T265.h"

Ano_T265 T265;

u8 _rx_buf[256];
uint8_t j=0;
int16_t raw_x,raw_y=0;
void T265_Get_Byte(u8 data)
{
	static u8 _sta = 0;
	static u8 cnt=2;
	
//	_rx_buf[j]=data;
//	j++;
//	if(j>200){j=0;}
	if(_sta==0&&data==0xFF)	//帧头0xFF
	{
		_sta=1;
		_rx_buf[0]=data;
	}
	else if(_sta==1&&data==0xFA)	//数据源，0xFA
	{
		_sta=2;
		_rx_buf[1]=data;
	}
	else if(_sta==2)		//12份数据
	{
		_rx_buf[cnt]=data;
		cnt++;
		if(cnt==8){_sta=3;cnt=2;}
	}
	else if(_sta==3&&data==0xFE)		//帧尾
	{
		raw_x=(uint16_t)(_rx_buf[2]<<8)+_rx_buf[3];
		if(_rx_buf[4]=='-'){raw_x=-raw_x;}
		raw_y=(uint16_t)(_rx_buf[5]<<8)+_rx_buf[6];
		if(_rx_buf[7]=='-'){raw_y=-raw_y;}
		
		T265.coordinate_x=-raw_y;
		T265.coordinate_y=raw_x;
		
//		T265.coordinate_x+=75;
//		T265.coordinate_y+=75;
		
//		T265.speed_x=(uint16_t)(_rx_buf[8]<<8)+_rx_buf[9];
//		if(_rx_buf[10]=='-'){T265.speed_x=-T265.speed_x;}
//		T265.speed_y=(uint16_t)(_rx_buf[11]<<8)+_rx_buf[12];
//		if(_rx_buf[13]=='-'){T265.speed_y=-T265.speed_y;}
	}
	else
		_sta = 0;
}




